#include "drv_motor.h"
#include "ThreadCtrl.h"
#include "drv_canthread.h"
#include <rtthread.h>
#include <rtdevice.h>
#include "pid.h"
#include <math.h>
// 低通滤波后把滤波后的角度用来PID计算

Motor_t motor_data;
Set_planner planner;                    // 设定值规划参量结构体
static struct rt_semaphore PID_sem;     /*线程信号量*/
static struct rt_timer PID_Timer;       /*线程定时器*/
static struct rt_timer Angle_Set_Timer; /*线程定时器*/

float vel_feed_gain = 1.0f; // 速度前馈增益，需要调试
float acc_feed_gain = 0.0f; // 加速度前馈增益，需要调试

static void PID_IRQHandler(void *parameter)
{
    rt_sem_release(&PID_sem);
}

static void Angle_Set_IRQHandler(void *parameter)
{
    float period = 0.3;
    float t = rt_tick_get() / 1000.f;
    float theta = fmod(t, period);
    // motor_data.ang.set = 4096 + 4096 * sinf(2 * PI * t);
    // motor_data.ang.set = 22753 * theta;
}

static void motor_init(Motor_t *motor, rt_uint32_t motorID, rt_int32_t Round_Len, float ratio, Crtl_e Mode, rt_int32_t Set_Max, rt_int32_t Set_Min)
{
    motor->dji.motorID = motorID;
    motor->dji.oldangle_state = RT_ERROR;
    motor->dji.ratio = ratio;
    motor->dji.Angle_CtrlMode = Mode;
    motor->dji.Round_Len = Round_Len;
    motor->dji.Set_MIN = Set_Min;
    motor->dji.Set_MAX = Set_Max;
    motor->dji.Data_Valid = 0; // 刚刚初始化完该结构体，置为无效，下次can通信收到的消息置为有效

    motor->dji.acceleration = 0;
    motor->dji.velocity = 0;
}

// 收到一个新的目标角度，调用此函数来规划一条新轨迹
void setting_plan(Set_planner *planner, float current_angle, float target_angle)
{
    planner->start_angle = current_angle;
    planner->target_angle = target_angle;
    planner->current_time = 0.0f;
    planner->status = 1; // 开始执行

    // 计算需要移动的距离
    float distance = fabsf(Motor_Get_DeltaAngle(planner->target_angle, planner->start_angle, 2 * PI));

    // 计算加速到最大速度所需的距离
    float acc_distance = (planner->max_vel * planner->max_vel) / (2.0f * planner->max_acc);

    // 判断是否能达到最大速度 (三角形还是梯形轨迹?)
    if (distance < 2.0f * acc_distance)
    {
        // 三角形轨迹：距离短，无法加速到最大速度
        planner->actual_vel = sqrtf(distance * planner->max_acc);
        planner->t_acc = planner->actual_vel / planner->max_acc;
        planner->t_peace = 0.0f;
        planner->t_dec = planner->t_acc;
    }
    else
    {
        // 梯形轨迹：距离足够，有一段匀速过程
        planner->actual_vel = planner->max_vel;
        planner->t_acc = planner->max_vel / planner->max_acc;
        planner->t_peace = (distance - 2.0f * acc_distance) / planner->max_vel;
        planner->t_dec = planner->t_acc;
    }

    planner->t_total = planner->t_acc + planner->t_peace + planner->t_dec;
}

// 每个控制周期被调用，根据当前时间给出规划的位置设定值、速度前馈和加速度前馈。
void setting_get_setpoint(Set_planner *planner)
{
    float t = planner->current_time;
    float flag = (planner->target_angle >= planner->start_angle) ? 1.0f : -1.0f;
    float period = 0.002f;
    // 初始化变量(第一次规划，时间设置为2ms)
    if (0 < t < 0.003)
    {
        planner->pos_setpoint = planner->start_angle;
        planner->vel_feedforward = 0.0f;
    }

    // 如果角度没变化，不需要改动
    if (planner->t_total <= 0)
    {
        planner->pos_setpoint = planner->target_angle;
        planner->vel_feedforward = 0.0f;
        planner->acc_feedforward = 0.0f;
    }

    // 根据运动阶段计算加速度
    if (0 < t < planner->t_acc)
    {

        planner->acc_feedforward = planner->max_acc * flag;
    }
    else if (t < planner->t_acc + planner->t_peace)
    {
        planner->acc_feedforward = 0.0f;
    }
    else if (t <= planner->t_total)
    {
        planner->acc_feedforward = -planner->max_acc * flag;
    }
    else
    {
        planner->acc_feedforward = 0.0f;
    }

    planner->vel_feedforward += planner->acc_feedforward * period;

    if (planner->vel_feedforward > planner->max_vel)
    {
        planner->vel_feedforward = planner->max_vel;
    }
    else if (planner->vel_feedforward < -planner->max_vel)
    {
        planner->vel_feedforward = -planner->max_vel;
    }

    planner->pos_setpoint += planner->vel_feedforward * period;
}

static void planner_init(Set_planner *planner)
{
    planner->target_angle = 0;
    planner->max_acc = 2000; // rad/s²
    planner->max_vel = 31;   // rad/s

    planner->actual_vel = 0.0f;
    planner->start_angle = 0.0f;
    planner->t_acc = 0.0f;
    planner->t_dec = 0.0f;
    planner->t_peace = 0.0f;
    planner->t_total = 0.0f;
    planner->status = 0;

    planner->acc_feedforward = 0.0f;
    planner->vel_feedforward = 0.0f;
    planner->pos_setpoint = 0.0f;
}
/**
 * @brief   电机初始化,对电机结构体进行初始化及pid参数初始化
 * @param   None
 * @return  rt_err_t 是否正常初始化
 */
rt_err_t
DJI_Motor_Init(void)
{
    motor_init(&motor_data, MOTOR_ID, ROUND_LEN, REDUCT_RATIO, LOOP_CTRL, 8192, 0); // 电机结构体dji初始化，包括减速比，编码器长度，最大最小设定值等的设置
    pid_init(&motor_data.spe, SPEED_PID_PARAMETER);                                 // pid参数初始化
    pid_init(&motor_data.ang, ANGLE_PID_PARAMETER);                                 // pid参数初始化
    planner_init(&planner);                                                         // 使用的设定值规划结构体初始化
    return RT_EOK;
}

static void Angle_PID_Calculate(void)
{
    static uint8_t runtimes = 0;
    if (motor_data.dji.Angle_CtrlMode == LOOP_CTRL && (rt_tick_get() - motor_data.dji.FreshTick < MOTOR_OFFLINE_TIME))
    {
        if (runtimes % PID_CTRL_EX_PERIOD == 0) // 角度环
        {
            if (fabsf(motor_data.ang.err) > ANGLE_PID_I_THREADHOLD) // 积分分离法，如果误差过大就逐渐减小积分项
                motor_data.ang.i_value *= I_CTRL_K;
            float angle_error = Motor_Get_DeltaAngle(motor_data.ang.set, motor_data.dji.angle, motor_data.dji.Round_Len);
            PID_Calculate(&motor_data.ang, angle_error);
            motor_data.spe.set = motor_data.ang.out; // 速度环的输入是角度环的输出
        }
        if (runtimes % PID_CTRL_IN_PERIOD == 0) // 速度环
        {
            if (fabsf(motor_data.spe.err) > SPEED_PID_I_THREADHOLD)
                motor_data.spe.i_value *= I_CTRL_K;
            float speed_error = motor_data.spe.set - motor_data.dji.speed;
            PID_Calculate(&motor_data.spe, speed_error);           // 速度环的输出是电流
            motor_data.spe.out += 293 + 72 * (motor_data.spe.set); // 速度环前馈
        }
    }
    runtimes++;
}

float vel_feed, acc_feed, pos_set;
static void motor_angle_setting_plan_PID(void)
{
    float period = 0.002f; // 运行周期为2ms

    // 基于时间进行规划，实时刷新时间
    if (planner.status == 1)
    {
        planner.current_time += period;
        if (planner.current_time > planner.t_total)
        {
            planner.status = 2;                     // 规划完成
            planner.current_time = planner.t_total; // 限制在总时间内
            // 规划完成后立即清零所有前馈相关变量
            planner.vel_feedforward = 0;
            planner.acc_feedforward = 0;
        }
    }

    setting_get_setpoint(&planner);
    pos_set = (planner.pos_setpoint / (2 * PI)) * 8192.f; // 从弧度rad转换为0-8192

    // 只规划中使用前馈
    //  只有规划进行中才使用前馈
    if (planner.status == 1)
    {
        vel_feed = ((planner.vel_feedforward * 30) / PI); // 速度转换为rpm
        // acc_feed = (planner.acc_feedforward / (2 * PI)) * 8192.f; // 加速度转换为编码器的值
    }
    else
    {
        vel_feed = 0;
        acc_feed = 0;
    }

    float angle_error = Motor_Get_DeltaAngle(motor_data.ang.set, motor_data.dji.angle, motor_data.dji.Round_Len);
    PID_Calculate(&motor_data.ang, angle_error);

    motor_data.ang.out += (vel_feed_gain * vel_feed);
}

// 加入前馈的角度环
static void Angle_Feedforward_PID_Calculate(void)
{
    static uint8_t runtimes = 0;
    if (motor_data.dji.Angle_CtrlMode == LOOP_CTRL && (rt_tick_get() - motor_data.dji.FreshTick < MOTOR_OFFLINE_TIME))
    {
        if (motor_data.ang.set != motor_data.ang.last_set)
        {
            float angle_now = (motor_data.dji.angle / 8192.f) * 2 * PI;  // 将实际的编码器反馈的角度转换为弧度
            float angle_target = (motor_data.ang.set / 8192.f) * 2 * PI; // 将设定值转换为弧度
            setting_plan(&planner, angle_now, angle_target);             // 当角度设定值有改变时，进行设定值规划，更新planner中的值
            motor_data.ang.last_set = motor_data.ang.set;
        }
        if (runtimes % PID_CTRL_EX_PERIOD == 0) // 角度环
        {
            // 设定值规划
            motor_angle_setting_plan_PID();
            if (fabsf(motor_data.ang.err) > ANGLE_PID_I_THREADHOLD) // 积分分离法，如果误差过大就逐渐减小积分项
                motor_data.ang.i_value *= I_CTRL_K;
            // float angle_error = Motor_Get_DeltaAngle(motor_data.ang.set, motor_data.dji.angle, motor_data.dji.Round_Len);//剩余的小部分误差交给pid
            // PID_Calculate(&motor_data.ang, angle_error);
            // motor_data.ang.out += output;

            // motor_data.dji.velocity = (motor_data.ang.set - motor_data.ang.last_set) / PID_CTRL_EX_PERIOD;               // 速度前馈（一阶导数）
            // motor_data.dji.acceleration = (motor_data.dji.velocity - motor_data.dji.last_velocity) / PID_CTRL_EX_PERIOD; // 加速度前馈（二阶导数）
            // motor_data.dji.last_velocity = motor_data.dji.velocity;
            // motor_data.ang.last_set = motor_data.ang.set;

            // //转1080°
            // motor_data.ang.set = (motor_data.ang.set_change *8192) /360;//将输入的以度为单位的转换为编码器的单位

            // motor_data.ang.out += 7.34 * motor_data.dji.velocity - 1;
            if (motor_data.ang.out > motor_data.ang.out_limit_up)
            {
                motor_data.ang.out = motor_data.ang.out_limit_up;
            }
            else if (motor_data.ang.out < motor_data.ang.out_limit_down)
            {
                motor_data.ang.out = motor_data.ang.out_limit_down;
            }
            motor_data.spe.set = motor_data.ang.out; // 速度环的输入是角度环的输出
        }
        if (runtimes % PID_CTRL_IN_PERIOD == 0) // 速度环
        {
            if (fabsf(motor_data.spe.err) > SPEED_PID_I_THREADHOLD)
                motor_data.spe.i_value *= I_CTRL_K;
            float speed_error = motor_data.spe.set - motor_data.dji.speed;
            PID_Calculate(&motor_data.spe, speed_error);           // 速度环的输出是电流
            motor_data.spe.out += 293 + 72 * (motor_data.spe.set); // 速度环前馈
        }
    }
    // motor_data.ang.set += 15;
    // if (motor_data.ang.set >= 8192)
    // {
    //     motor_data.ang.set = 0;
    // }
    runtimes++;
}

static void Speed_PID_TEST(void)
{
    if (fabsf(motor_data.spe.err) > SPEED_PID_I_THREADHOLD)
        motor_data.spe.i_value *= I_CTRL_K;
    float speed_err_test = motor_data.spe.set - motor_data.dji.speed;
    if (motor_data.dji.loop >= 3)
    {
        motor_data.spe.set = 0;
        speed_err_test = 0;
    }
    PID_Calculate(&motor_data.spe, speed_err_test); // 速度环的输出是电流
    motor_data.spe.out += 293 + 72 * (motor_data.spe.set);
}
static void Angle_PID_CanTrans()
{
    struct rt_can_msg txmsg;
    txmsg.id = MOTOR_SEND_ID; // 电压控制
    txmsg.ide = RT_CAN_STDID; /* 标准格式 */
    txmsg.rtr = RT_CAN_DTR;   /* 数据帧 */
    txmsg.len = 8;            /* 数据长度为 8 */

    txmsg.data[(motor_data.dji.motorID - MOTOR_ID_6020) * 2] = (uint8_t)((int32_t)(motor_data.spe.out) >> 8);
    txmsg.data[(motor_data.dji.motorID - MOTOR_ID_6020) * 2 + 1] = (rt_uint8_t)((int32_t)(motor_data.spe.out));

    rt_device_write(can1_dev, 0, &txmsg, sizeof(txmsg));
}

static void PID_Cal_Thread(void)
{
    while (1)
    {
        rt_sem_take(&PID_sem, RT_WAITING_FOREVER); // 1ms就take一次该信号量，即1ms就可以完成一次pid计算
        // Angle_PID_Calculate();
        //     Speed_PID_Calculate();
        //  Speed_PID_TEST();
        Angle_Feedforward_PID_Calculate();
        Angle_PID_CanTrans();
        // Speed_PID_CanTrans();
    }
}

// 初始化电机结构体及创建PID线程
rt_err_t MOTOR_THREAD_Init(void)
{
    rt_err_t res = DJI_Motor_Init();
    if (res != RT_EOK)
    {
        return RT_ERROR;
    }

    // 初始化信号量
    rt_sem_init(&PID_sem, "PID_Sem", 0, RT_IPC_FLAG_FIFO);

    rt_thread_t thread = rt_thread_create("PID_Cal_Thread", PID_Cal_Thread, RT_NULL, THREAD_STACK_PID, THREAD_PRIO_PID, THREAD_TICK_PID);
    if (thread == RT_NULL)
    {
        return RT_ERROR;
    }

    res = rt_thread_startup(thread);
    if (res != RT_EOK)
    {
        return RT_ERROR;
    }

    // 创建线程定时器 定时释放信号量 PID_Sem
    rt_timer_init(&PID_Timer,
                  "PID_Timer",
                  PID_IRQHandler,
                  RT_NULL,
                  1,
                  RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);

    // 启动定时器
    res = rt_timer_start(&PID_Timer);
    if (res != RT_EOK)
        return res;

    // 创建定时器 定时设置新的角度设定值
    rt_timer_init(&Angle_Set_Timer,
                  "Angle_Set_Timer",
                  Angle_Set_IRQHandler,
                  RT_NULL,
                  1,
                  RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);

    // 启动定时器
    res = rt_timer_start(&Angle_Set_Timer);
    if (res != RT_EOK)
        return res;
    return RT_EOK;
}